
using UnityEngine;
//using UnityEditor;
using System;

public class PhysicsObject : MonoBehaviour
{
	private Vector3 destinationPosition;
	
		
	private bool doCollisionAvoidance= true;
	
	// Motion Varibles
	private float maxTorque = 8000.0f;
	private float maxForwardForce = 2500.0f;
	private float distanceOfMaxForce = 150.0f;
	private float angleOfMaxTorque = 20.0f;
	
	private float angularDrag = 10.0f;
	private float drag = 6.0f;
	
	// Collision Varibles
	private float antennaAngle = 85.0f;
	private int antennaNumOfRays = 4;
	private float antennaLengthMin = 35.0f;
	private float antennaLengthVelocityFactor = 0.4f;
	private int antennaSweepDivisions = 14;
	private float antennaSweepAngle = 280.0f; 
	
	public void init ()
	{
		if (!gameObject	.GetComponent("Rigidbody"))
			gameObject.AddComponent("Rigidbody");
		setDestination(getPosition());
		
		// (Freeze x and z rotation and freeze y position, so they stick to the 2d surface)
		//  and no gravity)
		rigidbody.constraints = RigidbodyConstraints.FreezePositionY 
			| RigidbodyConstraints.FreezeRotationX
			| RigidbodyConstraints.FreezeRotationZ;
		rigidbody.useGravity = false;
		
		rigidbody.angularDrag = angularDrag;
		rigidbody.drag = drag;
	}
	
	// Messy but oh well
	public void SetValues(float maxTorque, float maxForwardForce, float distanceOfMaxForce,
	 float angleOfMaxTorque, float angularDrag, float drag,
	 float antennaAngle, int antennaNumOfRays, float antennaLengthMin,
	 float antennaLengthVelocityFactor, int antennaSweepDivisions, float antennaSweepAngle)
	{
			// Motion Varibles
		this.maxTorque = maxTorque;
		this.maxForwardForce = maxForwardForce;
		this.distanceOfMaxForce = distanceOfMaxForce;
		this.angleOfMaxTorque = angleOfMaxTorque;
			
		this.angularDrag = angularDrag;
		this.drag = drag;
			
			// Collision Varibles
		this.antennaAngle = antennaAngle;
		this.antennaNumOfRays = antennaNumOfRays;
		this.antennaLengthMin = antennaLengthMin;
		this.antennaLengthVelocityFactor = antennaLengthVelocityFactor;
		this.antennaSweepDivisions = antennaSweepDivisions;
		this.antennaSweepAngle = antennaSweepAngle; 
	  
		rigidbody.angularDrag = angularDrag;
		rigidbody.drag = drag;
	}
	
	public void setMass(float newMass)
	{
		rigidbody.mass = newMass;	
	}


	public void updatePhysics () 
	{	
		//Debug.DrawLine (getPosition(), getDestination(), Color.white);
	    if (doCollisionAvoidance)
			avoidCollisions();
		applyForces();
	}	

	public void setDoCollisions(bool doCollisions)
	{
		this.doCollisionAvoidance = doCollisions;	
	}
	
	public void setMaxTrust(float maxThrust)
	{
		this.maxForwardForce = maxThrust;	
	}
	
	private void applyForces()
	{
		if (!rigidbody)
			return;
		// Get the target vector treating the agent as the origin
		Vector3 targetVector = destinationPosition - rigidbody.position;
		
		// get the rotation of the agent
		double agentRotation = rigidbody.rotation.eulerAngles.y;
		
		// Avoid a divide by zero
		if (targetVector.z == 0)
			targetVector.z = 0.01f;
		
		// get the angle from the agent to the target
		double targetAngle = (180/Math.PI) * Math.Atan(targetVector.x / targetVector.z);

		if (targetVector.x > 0 && targetVector.z < 0)
			targetAngle += 180;
		else if (targetVector.x < 0 && targetVector.z < 0)
			targetAngle = -180 + targetAngle;
		
		// get the difference of angles - we can then use
		// it as a scalar with addTorque
		double angleDelta = targetAngle - agentRotation;
		while (angleDelta > 180)
			angleDelta -= 360;
		while (angleDelta < -180)
			angleDelta += 360;
				
		double forceScalar;
		
		double distanceForceScalar;
		if (targetVector.magnitude > distanceOfMaxForce)
			distanceForceScalar = 1.0;
		else
			distanceForceScalar = targetVector.magnitude / distanceOfMaxForce;
		
		if (Math.Abs(angleDelta) > 90)
			forceScalar = 0.0f;
		else 
			forceScalar = (1.0f - (Math.Abs(angleDelta)/(90.0f)))
				* distanceForceScalar 
				* maxForwardForce;
		
		Vector3 appliedForce = Vector3.forward * (float)forceScalar;
		rigidbody.AddRelativeForce(appliedForce);
		
		double angleTorqueScalar;
		
		if (Math.Abs(angleDelta) > angleOfMaxTorque)
			angleTorqueScalar = 1;
		else
			angleTorqueScalar = Math.Abs(angleDelta) / angleOfMaxTorque;
		
		double torqueScalar = angleTorqueScalar * (angleDelta/180) * maxTorque;
		Vector3 appliedTorque = new Vector3(0.0f,(float)(torqueScalar),0.0f);
		rigidbody.AddTorque(appliedTorque);

	}
	
	
	private void avoidCollisions()
	{
		
		Vector3 herderPos = getPosition();
		//float antennaAngle = Vector3.Angle(getPosition(), getDestination());	
		if (!rigidbody)
			return;
		if (!antennaHitting(rigidbody.rotation.eulerAngles.y))
			return; // no collison
			
		float tryAngle = 0;
		
		//Which direction to try first
		bool positive = UnityEngine.Random.value > 0.5f;
		int rayCount = 0;	
		do
		{
			rayCount++;	
			if (rayCount > antennaSweepDivisions)
			{
				setDestination(transform.TransformDirection(Vector3.back) * 30); // turn around
				return;
			}

			tryAngle = rigidbody.rotation.eulerAngles.y;
			//tryAngle = Vector3.Angle(getPosition(), getDestination());
			float angleToAdd = rayCount * ((antennaSweepAngle/2)/antennaSweepDivisions);
			
			if (positive)
			{
				tryAngle += angleToAdd; 
			} else
			{
				tryAngle -= angleToAdd;
			}
			positive = !positive;	
			
		} while(antennaHitting(tryAngle));
	
		Vector3 newDest = herderPos;
		float destDistance = (herderPos - getDestination()).magnitude;
		
		newDest.x = herderPos.x + (destDistance) * (float)Math.Sin( (Math.PI/180) * tryAngle);
		newDest.z = herderPos.z + (destDistance) * (float)Math.Cos( (Math.PI/180) * tryAngle);	
		
		setDestination(newDest);
		
	}
	
	bool antennaHitting(float angle)
	{
	
		//RaycastHit hitInfo = new RaycastHit();
		float rayAngle;
		Vector3 direction = new Vector3(0,0,0);
		for (int i = 0; i < antennaNumOfRays; i++)
		{
			rayAngle = angle -(antennaAngle/2)+(i*(antennaAngle/antennaNumOfRays));
			direction.x = (float)Math.Sin( (Math.PI/180) * rayAngle );
			direction.z = (float)Math.Cos( (Math.PI/180) * rayAngle );
			direction.Normalize();
			
			float lengthScalar = 1 - Math.Abs( ((float)i)/antennaNumOfRays - 0.5f );
			
			RaycastHit[] hits;
	        //hits = Physics.RaycastAll(direction, transform.forward, 100.0F);
	        hits = Physics.RaycastAll(getPosition(), 
					direction,
					lengthScalar * (antennaLengthMin + antennaLengthVelocityFactor*rigidbody.velocity.magnitude));
			
			int j = 0;
	        while (j < hits.Length) {
	            RaycastHit hit = hits[j];
	            GameObject obj = hit.collider.gameObject;
	            if (!obj.GetComponent("AgentBehavoirScript"))
				{
					return true;
				}
				j++;
	        }
			
			/*if (Physics.Raycast(getPosition(), 
					direction,
					out hitInfo, 
					lengthScalar * (antennaLengthMin + antennaLengthVelocityFactor*rigidbody.velocity.magnitude))
			    )
			{*/
				//Debug.DrawLine(getPosition(), hitInfo.point, Color.red);
				//if (!hitInfo.rigidbody)
				//return true;
			

			Debug.DrawLine(getPosition(), getPosition()+(direction*( lengthScalar * (antennaLengthMin + antennaLengthVelocityFactor*rigidbody.velocity.magnitude))), Color.blue);
		}
		return false;
	}	
	
		
	public Vector3 getPosition ()
	{
		return transform.position;
	}
	
	public void setDestination(Vector3 position)
	{
		destinationPosition = position;
	}
	
	public Vector3 getDestination()
	{
		return destinationPosition;
	}	
		
	
}

